#include <Wire.h>

const int iCompassI2C = 0x60;

float body_compass_bearing=0.0f;
float body_compass_pitch=0.0f;
float body_compass_roll=0.0f;

void get_body_compass(void)
{
  byte high, low, fine;              // highByte and lowByte store high and low bytes of the bearing and fine stores decimal place of bearing
  char pitch, roll;                          // Stores pitch and roll values of CMPS09, chars are used because they support signed value
  int bearing;                               // Stores full bearing

    Wire.beginTransmission(iCompassI2C);           //starts communication with CMPS09
  Wire.write(2);                              //Sends the register we wish to start reading from
  Wire.endTransmission();

  Wire.requestFrom(iCompassI2C, 4);              // Request 4 bytes from CMPS09
  while(Wire.available() < 4);               // Wait for bytes to become available
  high = Wire.read();
  low = Wire.read();
  pitch = Wire.read();
  roll = Wire.read();

  bearing = ((high<<8)+low);         // Calculate decimal place of bearing

  body_compass_bearing = bearing/10.f;
  body_compass_pitch = pitch;
  body_compass_roll = roll;
}

void print_compass(void)
{
  get_body_compass();
  Serial.print("Bearing : ");
  Serial.println(body_compass_bearing);   
  Serial.print("Pitch : ");
  Serial.println(body_compass_pitch);   
  Serial.print("Roll : ");
  Serial.println(body_compass_roll);   
}

void calibration(void)
{
  while(Serial.available()>0){
    Serial.read();
  } // clear buffer :)
  Serial.println("This is begin compass calibration");
  Wire.beginTransmission(iCompassI2C);
  Wire.write(22);
  Wire.write(0xF0);
  Wire.endTransmission();  

  Serial.println("Place your compass to 0 deg (North) and hit a key");
  while(Serial.available()==0); // wait input from user !
  while(Serial.available()>0){
    Serial.read();
  } // clear buffer :)
  Wire.beginTransmission(iCompassI2C);
  Wire.write(22);
  Wire.write(0xF5);
  Wire.endTransmission();  

  Serial.println("Place your compass to 90 deg (East) and hit a key");
  while(Serial.available()==0); // wait input from user !
  while(Serial.available()>0){
    Serial.read();
  } // clear buffer :)
  Wire.beginTransmission(iCompassI2C);
  Wire.write(22);
  Wire.write(0xF5);
  Wire.endTransmission();

  Serial.println("Place your compass to 180 deg (South) and hit a key");
  while(Serial.available()==0); // wait input from user !
  while(Serial.available()>0){
    Serial.read();
  } // clear buffer :)
  Wire.beginTransmission(iCompassI2C);
  Wire.write(22);
  Wire.write(0xF5);
  Wire.endTransmission();  

  Serial.println("Place your compass to 270 deg (West) and hit a key");
  while(Serial.available()==0); // wait input from user !
  while(Serial.available()>0){
    Serial.read();
  } // clear buffer :)
  Wire.beginTransmission(iCompassI2C);
  Wire.write(22);
  Wire.write(0xF5);
  Wire.endTransmission();
  Serial.println("Calibration finished.");
}

void default_calibration(void)
{
  Serial.println("This is resetting compass calibration to default");
  Wire.beginTransmission(iCompassI2C);
  Wire.write(22);
  Wire.write(0x20);
  Wire.endTransmission();
  delay(100);
  Wire.beginTransmission(iCompassI2C);
  Wire.write(22);
  Wire.write(0x2A);
  Wire.endTransmission();
  delay(100);
  Wire.beginTransmission(iCompassI2C);
  Wire.write(22);
  Wire.write(0x60);
  Wire.endTransmission();
}

void print_version(void)
{
  int data; // Software version of CMPS10 is read into data and then returned

  Wire.beginTransmission(iCompassI2C);
  Wire.write((byte)0); // Sends the register we wish to start reading from
  Wire.endTransmission();

  Wire.requestFrom(iCompassI2C, 1); // Request byte from CMPS10
  while(Wire.available() < 1);
  data = Wire.read();

  Serial.print("Version :");
  Serial.println(data, HEX);
}

void print_cmd(void)
{
}

void setup(void)
{
  Serial.begin(9600);
  Wire.begin();
  print_version();
  print_cmd();   
}

boolean _loop_compass_ = false;
unsigned long _compass_timing_ = 0;

void loop(void)
{
  if(_loop_compass_)
  {
    unsigned long _time_ = millis();
    if((_time_-_compass_timing_)>250)
    {
      _compass_timing_ = _time_;
      print_compass();
    }
  }
  if(Serial.available()>0)
  {
    int cmd = Serial.read();
    Serial.print("cmd=");
    Serial.println(cmd);    

    if(cmd == 'c' || cmd == 'C')
    {
      // launch calibration
      calibration();
    }
    else if(cmd == 'r' || cmd == 'R')
    {
      // reset to default calibration
      default_calibration();
    }
    else if(cmd == 'd' || cmd == 'D')
    {
      // print data
      print_compass();
    }
    else if(cmd == 'v' || cmd == 'V')
    {
      // print data
      print_version();
    }
    else if(cmd == 'l' || cmd == 'L')
    {
      // turn off/on loop
      _loop_compass_ = !_loop_compass_;
    }
    else// if(cmd == 'i' || cmd == 'I')
    {
      // print cmd
      print_cmd();
    }
  }
}

